#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <linux/types.h>
#include <linux/videodev2.h>

#define FILE_VIDEO "/dev/video0"
#define BMP        "/tmp/image_bmp.bmp"
#define YUV        "/tmp/image_yuv.yuv"     /* yuvviewer */

#define IMAGEWIDTH  640
#define IMAGEHEIGHT 480

static int fd;
static struct v4l2_capability cap;
struct v4l2_fmtdesc fmtdesc;
struct v4l2_format fmt, fmtack;
struct v4l2_streamparm setfps;
struct v4l2_requestbuffers req;
struct v4l2_buffer buf;
enum v4l2_buf_type type;
unsigned char frame_buffer[IMAGEWIDTH * IMAGEHEIGHT * 3];


/*
 * init
 */
int init_v4l2(void)
{
    int i;
    int ret = 0;

    /* open */
    if ((fd = open(FILE_VIDEO, O_RDWR)) == -1) {
        printf("error opening v4l interface...\n");
    }

    /* read video_capability attr */
    if (ioctl(fd, VIDIOC_QUERYCAP, &cap) == -1) {
        printf("error opening device %s: unable to query device.\n", FILE_VIDEO);
        return -1;
    }
    else {
        printf("driver:\t\t%s\n", cap.driver);
        printf("card:\t\t%s\n", cap.card);
        printf("bus_info:\t%s\n", cap.bus_info);
        printf("version:\t%d\n", cap.version);
        printf("capabilities:\t%x\n", cap.capabilities);

        if ((cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) == V4L2_CAP_VIDEO_CAPTURE) {
            printf("device %s: supports capture.\n", FILE_VIDEO);
        }

        if ((cap.capabilities & V4L2_CAP_STREAMING) == V4L2_CAP_STREAMING) {
            printf("device %s: supports streaming.\n", FILE_VIDEO);
        }
    }

    fmtdesc.index = 0;
    fmtdesc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    printf("support format:\n");

    /* 
     * 列举摄像头所支持像素格式.
     * 使用命令VIDIOC_ENUM_FMT，获取到的信息通过结构体v4l2_fmtdesc查询。这步很关键，
     * 不同的摄像头可能支持的格式不一样，
     * V4L2可以支持的格式很多，/usr/include/linux/videodev2.h文件中可以看到 
     */
    while (ioctl(fd, VIDIOC_ENUM_FMT, &fmtdesc) != -1) {
        printf("\t%d.%s\n", fmtdesc.index + 1, fmtdesc.description);
        fmtdesc.index++;
    }

    /* 设置像素格式 */
    fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
    fmt.fmt.pix.height = IMAGEHEIGHT;
    fmt.fmt.pix.width = IMAGEWIDTH;
    fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;

    if (ioctl(fd, VIDIOC_S_FMT, &fmt) == -1) {
        printf("unable to set format.\n");
        return -1;
    }

    /* 确保设置的格式作用到摄像头上 */
    if (ioctl(fd, VIDIOC_G_FMT, &fmt) == -1) {
        printf("unable to get format.\n");
        return -1;
    }

    printf("fmt.type:\t\t%d\n", fmt.type);
    printf("pix.pixelformat:\t%c%c%c%c\n", fmt.fmt.pix.pixelformat & 0xff,
           (fmt.fmt.pix.pixelformat >> 8) & 0xff, 
           (fmt.fmt.pix.pixelformat >> 16) & 0xff,
           (fmt.fmt.pix.pixelformat >> 24) & 0xff);
    printf("pix.height:\t\t%d\n", fmt.fmt.pix.height);
    printf("pix.width:\t\t%d\n", fmt.fmt.pix.width);
    printf("pix.field:\t\t%d\n", fmt.fmt.pix.field);

    /* set fps */
    setfps.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    setfps.parm.capture.timeperframe.numerator = 10;
    setfps.parm.capture.timeperframe.denominator = 10;

    printf("init %s \t[ok]\n", FILE_VIDEO);

    return 0;
}

/*
 * 获取每个缓存的信息，并mmap到用户空间。定义结构体
 *
 * 来存储mmap后的地址信息。需要说明的是由于mmap函数定义时返回的地址是个void *，
 * 因而这里面的start也是个 void *。实际地址在运行的时候会自动分配。 
 */
struct buffer 
{
    void * start;
    unsigned int length;
} * buffers;

/*
 * 图像采集
 */
int v4l2_grab(void)
{
    unsigned int n_buffers;

    /*
     * 请缓存区
     */
    req.count = 4;
    req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    req.memory = V4L2_MEMORY_MMAP;
    if (ioctl(fd, VIDIOC_REQBUFS, &req) == -1) {
        printf("request for buffers error.\n");
        return -1;
    }

    /* mmap for buffers */
    buffers = malloc(req.count * sizeof(*buffers));
    if (!buffers) {
        printf("out of memory.\n");
        return -1;
    }

    for (n_buffers = 0; n_buffers < req.count; n_buffers++) {
        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory = V4L2_MEMORY_MMAP;
        buf.index = n_buffers;

        /* query buffers */
        if (ioctl(fd, VIDIOC_QUERYBUF, &buf) == -1) {
            printf("query buffer error.\n");
            return -1;
        }

        buffers[n_buffers].length = buf.length;

        /* map */
        buffers[n_buffers].start = mmap(NULL, buf.length, 
                                        PROT_READ | PROT_WRITE, 
                                        MAP_SHARED,
                                        fd, buf.m.offset);

        if (buffers[n_buffers].start == MAP_FAILED) {
            printf("buffer map error.\n");
            return -1;
        }
    }

    /* queue */
    for (n_buffers = 0; n_buffers < req.count; n_buffers++) {
        buf.index = n_buffers;
        ioctl(fd, VIDIOC_QBUF, &buf);
    }

    /* 开始采集视频 */
    type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    ioctl(fd, VIDIOC_STREAMON, &type);

    /* 取出缓存中已经采样的缓存。使用命令VIDIOC_DQBUF。
       视频数据存放的位置是buffers[n_buffers].start的地址处 */
    ioctl(fd, VIDIOC_DQBUF, &buf);

    printf("grap yuyv ok.\n");

    return 0;
}

#define check_value(v) ({if (v > 255) v = 255; else if (v < 0) v = 0;})

/*
 * YUYV转RGB24
 *
 * 由于采集到的图像数据是YUYV格式，需要进行颜色空间转换，定义了转换函数
 *
 * 由于摄像头采集的数据格式为YUYV，为了方便后续设计，需要转变为RGB24，
 * 并将转换完成的数据存储到frame_buffer中。
 * 值得一提的是，由于定义的时候buffers[index].start是个void *，
 * 没有办法进行+1这样的操作，需要强制转换
 */
int yuyv_2_rgb888(void)
{
    int i, j;
    unsigned char y1, y2, u, v;
    int r1, g1, b1, r2, g2, b2;
    char * pointer;

    pointer = buffers[0].start;

    for (i = 0; i < 480; i++) {
        for (j = 0; j < 320; j++) {
            y1 = *(pointer + (i * 320 + j) * 4);
            u =  *(pointer + (i * 320 + j) * 4 + 1);
            y2 = *(pointer + (i * 320 + j) * 4 + 2);
            v =  *(pointer + (i * 320 + j) * 4 + 3);

            r1 = y1 + 1.042 * (v - 128);
            g1 = y1 - 0.34414 * (u - 128) - 0.71414 * (v - 128);
            b1 = y1 + 1.772 * (u - 128);

            r2 = y2 + 1.042 * (v - 128);
            g2 = y2 - 0.34414 * (u - 128) - 0.71414 * (v - 128);
            b2 = y2 + 1.772 * (u - 128);

            if (r1 > 255)
                r1 = 255;
            else if (r1 < 0)
                r1 = 0;

            if (b1 > 255)
                b1 = 255;
            else if (b1 < 0)
                b1 = 0;
            
            check_value(g1);
            check_value(r2);
            check_value(b2);
            check_value(g2);

            *(frame_buffer + ((480 - 1 - i) * 320 + j) * 6) = (unsigned char) b1;
            *(frame_buffer + ((480 - 1 - i) * 320 + j) * 6 + 1) = (unsigned char) g1;
            *(frame_buffer + ((480 - 1 - i) * 320 + j) * 6 + 2) = (unsigned char) r1;
            *(frame_buffer + ((480 - 1 - i) * 320 + j) * 6 + 3) = (unsigned char) b2;
            *(frame_buffer + ((480 - 1 - i) * 320 + j) * 6 + 4) = (unsigned char) g2;
            *(frame_buffer + ((480 - 1 - i) * 320 + j) * 6 + 5) = (unsigned char) r2;
        }
    }

    printf("change to rgb ok.\n");
}

int close_v4l2(void)
{
    // ioctl(fd, VIDIOC_STREAMOFF, &buf_type);
    if (fd != -1) {
        close(fd);
        return 0;
    }

    return -1;
}

/*
 * 采集到图像数据存储成图片，为了方便调试，先将原始的数据存储为yuv格式文件，
 * 再将转换成RGB后的数据存储为BMP。
 *
 * 定义BMP头结构体
 */
typedef unsigned short   WORD;  /* 16bit or 2byte */
typedef unsigned int    DWORD;  /* 32bit or 4byte */

struct bitmap_file_header
{
    WORD bf_type;    /* the flag of bmp, value is "BM" */
    DWORD bf_size;   /* size bmp file, unit is bytes */
    DWORD bf_reserved;   /* 0 */
    DWORD bf_offbits;    /* must be 54 */
};

struct bitmap_info_header
{
    DWORD bi_size;   /* must be 0x28 */
    DWORD bi_width;
    DWORD bi_height;
    WORD bi_planes;  /* must be 1 */
    WORD bi_bit_count;
    DWORD bi_compression;
    DWORD bi_size_image;
    DWORD bi_xpels_per_meter;
    DWORD bi_ypels_per_meter;
    DWORD bi_clr_used;
    DWORD bi_clr_important;
};

int main()
{
    FILE * fp1, * fp2;
    struct bitmap_file_header bf;
    struct bitmap_info_header bi;

    fp1 = fopen(BMP, "wb");
    if (!fp1) {
        printf("open bmp file error.\n");
        return -1;
    }

    fp2 = fopen(YUV, "wb");
    if (!fp2) {
        printf("open yuv file error.\n");
        return -1;
    }

    if (init_v4l2() == -1)
        return -1;

    bi.bi_size = 40;
    bi.bi_width = IMAGEWIDTH;
    bi.bi_height = IMAGEHEIGHT;
    bi.bi_planes = 1;
    bi.bi_bit_count = 24;
    bi.bi_size_image = IMAGEWIDTH * IMAGEHEIGHT * 3;
    bi.bi_xpels_per_meter = 0;
    bi.bi_ypels_per_meter = 0;
    bi.bi_clr_used = 0;
    bi.bi_clr_important = 0;

    bf.bf_type = 0x4d42;
    bf.bf_size = 52 + bi.bi_size_image;
    bf.bf_reserved = 0;
    bf.bf_offbits = 54;

    v4l2_grab();
    fwrite(buffers[0].start, 640*480*2, 1, fp2);
    printf("save yuv ok.\n");

    yuyv_2_rgb888();
    fwrite(&bf, 14, 1, fp1);
    fwrite(&bi, 40, 1, fp1);
    fwrite(frame_buffer, bi.bi_size_image, 1, fp1);
    printf("save bmp ok.\n");

    fclose(fp1);
    fclose(fp2);
    close_v4l2();

    return 0;
}
